<!-- saved from url=(0014)about:internet -->
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1"> 1</a></span><span class="line"><span class="keyword">function</span> [ <span class="var type1" id="S2T9U3">h</span> <span class="var type1" id="S3T9U4">v</span> <span class="var type1" id="S4T9U5">a</span> <span class="var type1" id="S5T9U6">a_a</span>] = nav_h_ekf(<span class="var type1" id="S6T2U9">Q</span>, <span class="var type1" id="S7T13U10">H</span>, <span class="var type1" id="S8T15U11">R</span> ,<span class="var type1" id="S9T2U12">F</span>, <span class="var type1" id="S10T2U13">I</span> ,<span class="var type1" id="S11T19U14">Z</span>,<span class="var type1" id="S12T12U15">up</span> )</span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">%作者 ytf</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3"> 3</a></span><span class="line"><span class="comment">%日期 2017年4月27日</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4"> 4</a></span><span class="line"><span class="comment">%状态转移协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">%H 预测矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">%R 观测噪声方差</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%F 状态转移矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%I</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%Z 测量矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10">10</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11">11</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S13T3U17">X</span> ;<span class="comment">% 状态变量</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12">12</a></span><span class="line">          <span class="keyword">if</span>(<span class="mxinfo" id="T1:U13"><span class="mxinfo" id="T1:U14">isempty(<span class="var type0" id="S13T0U23">X</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13">13</a></span><span class="line">           <span class="mxinfo" id="T3:U15"><span class="var type1" id="S13T3U26">X</span>=<span class="mxinfo" id="T3:U17">single(<span class="mxinfo" id="T21:U18">[0 0 0 0]'</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14">14</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15">15</a></span><span class="line">          </span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16">16</a></span><span class="line"> <span class="keyword">persistent</span> <span class="var type1" id="S16T2U37">P</span> ;<span class="comment">% 状态协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17">17</a></span><span class="line">            <span class="keyword">if</span>(<span class="mxinfo" id="T1:U20"><span class="mxinfo" id="T1:U21">isempty(<span class="var type0" id="S16T0U43">P</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18">18</a></span><span class="line">            <span class="mxinfo" id="T2:U22"><span class="var type1" id="S16T2U46">P</span>=<span class="mxinfo" id="T2:U24">single(<span class="mxinfo" id="T22:U25">[10 0 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19">19</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T22:U25">                     0 10 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20">20</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T22:U25">                     0 0 10 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21">21</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T22:U25">                     0 0 0  100]</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22">22</a></span><span class="line">            <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23">23</a></span><span class="line">       <span class="mxinfo" id="T3:U26"><span class="var type1" id="S13T3U72">X</span>=<span class="mxinfo" id="T3:U28"><span class="var type1" id="S9T2U74">F</span>*<span class="var type1" id="S13T3U75">X</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24">24</a></span><span class="line">       <span class="mxinfo" id="T2:U31"><span class="var type1" id="S16T2U78">P</span>=<span class="mxinfo" id="T2:U33"><span class="mxinfo" id="T2:U34"><span class="mxinfo" id="T2:U35"><span class="var type1" id="S9T2U82">F</span>*<span class="var type1" id="S16T2U83">P</span></span>*<span class="mxinfo" id="T2:U38"><span class="var type1" id="S9T2U85">F</span>'</span></span>+<span class="var type1" id="S6T2U86">Q</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25">25</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U41"><span class="var type1" id="S12T12U90">up</span>==<span class="mxinfo" id="T6:U43">1</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26">26</a></span><span class="line">       <span class="mxinfo" id="T14:U44"><span class="var type1" id="S17T14U94">K</span>=<span class="mxinfo" id="T14:U46"><span class="mxinfo" id="T14:U47"><span class="var type1" id="S16T2U97">P</span>*<span class="mxinfo" id="T14:U49"><span class="var type1" id="S7T13U99">H</span>'</span></span>/(<span class="mxinfo" id="T15:U51"><span class="mxinfo" id="T15:U52"><span class="mxinfo" id="T13:U53"><span class="var type1" id="S7T13U104">H</span>*<span class="var type1" id="S16T2U105">P</span></span>*<span class="mxinfo" id="T14:U56"><span class="var type1" id="S7T13U107">H</span>'</span></span> +<span class="var type1" id="S8T15U108">R</span></span>)</span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27">27</a></span><span class="line">       <span class="mxinfo" id="T3:U59"><span class="var type1" id="S13T3U111">X</span>=<span class="mxinfo" id="T3:U61"><span class="var type1" id="S13T3U113">X</span>+<span class="mxinfo" id="T3:U63"><span class="var type1" id="S17T14U115">K</span>*(<span class="mxinfo" id="T19:U65"><span class="var type1" id="S11T19U118">Z</span>-<span class="mxinfo" id="T19:U67"><span class="var type1" id="S7T13U120">H</span>*<span class="var type1" id="S13T3U121">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28">28</a></span><span class="line">       <span class="mxinfo" id="T2:U70"><span class="var type1" id="S16T2U124">P</span>=<span class="mxinfo" id="T2:U72">(<span class="mxinfo" id="T2:U73">single(<span class="var type1" id="S10T2U130">I</span>)-<span class="mxinfo" id="T2:U75"><span class="var type1" id="S17T14U132">K</span>*<span class="var type1" id="S7T13U133">H</span></span></span>)*<span class="var type1" id="S16T2U134">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29">29</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30">30</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31">31</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U79"><span class="var type1" id="S12T12U138">up</span>==<span class="mxinfo" id="T6:U81">0</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,32" id="srcline32">32</a></span><span class="line">       <span class="mxinfo" id="T20:U82"><span class="var type1" id="S18T20U142">H_H</span>=<span class="mxinfo" id="T20:U84"><span class="var type1" id="S7T13U144">H</span>(<span class="mxinfo" id="T6:U86">2</span>,:)</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,33" id="srcline33">33</a></span><span class="line">       <span class="mxinfo" id="T3:U87"><span class="var type1" id="S17T3U149">K</span>=<span class="mxinfo" id="T3:U89"><span class="mxinfo" id="T3:U90"><span class="var type1" id="S16T2U152">P</span>*<span class="mxinfo" id="T3:U92"><span class="var type1" id="S18T20U154">H_H</span>'</span></span>/(<span class="mxinfo" id="T9:U94"><span class="mxinfo" id="T9:U95"><span class="mxinfo" id="T20:U96"><span class="var type1" id="S18T20U159">H_H</span>*<span class="var type1" id="S16T2U160">P</span></span>*<span class="mxinfo" id="T3:U99"><span class="var type1" id="S18T20U162">H_H</span>'</span></span> +<span class="mxinfo" id="T9:U101"><span class="var type1" id="S8T15U164">R</span>(<span class="mxinfo" id="T6:U103">2</span>,<span class="mxinfo" id="T6:U104">2</span>)</span></span>)</span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,34" id="srcline34">34</a></span><span class="line">       <span class="mxinfo" id="T3:U105"><span class="var type1" id="S13T3U169">X</span>=<span class="mxinfo" id="T3:U107"><span class="var type1" id="S13T3U171">X</span>+<span class="mxinfo" id="T3:U109"><span class="var type1" id="S17T3U173">K</span>*(<span class="mxinfo" id="T9:U111"><span class="mxinfo" id="T9:U112"><span class="var type1" id="S11T19U177">Z</span>(<span class="mxinfo" id="T6:U114">2</span>)</span>-<span class="mxinfo" id="T9:U115"><span class="var type1" id="S18T20U180">H_H</span>*<span class="var type1" id="S13T3U181">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,35" id="srcline35">35</a></span><span class="line">       <span class="mxinfo" id="T2:U118"><span class="var type1" id="S16T2U184">P</span>=<span class="mxinfo" id="T2:U120">(<span class="mxinfo" id="T2:U121">single(<span class="var type1" id="S10T2U190">I</span>)-<span class="mxinfo" id="T2:U123"><span class="var type1" id="S17T3U192">K</span>*<span class="var type1" id="S18T20U193">H_H</span></span></span>)*<span class="var type1" id="S16T2U194">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,36" id="srcline36">36</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,37" id="srcline37">37</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,38" id="srcline38">38</a></span><span class="line">       <span class="mxinfo" id="T9:U127"><span class="var type1" id="S2T9U197">h</span>=<span class="mxinfo" id="T9:U129"><span class="var type1" id="S13T3U199">X</span>(<span class="mxinfo" id="T6:U131">1</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,39" id="srcline39">39</a></span><span class="line">       <span class="mxinfo" id="T9:U132"><span class="var type1" id="S3T9U203">v</span>=<span class="mxinfo" id="T9:U134"><span class="var type1" id="S13T3U205">X</span>(<span class="mxinfo" id="T6:U136">2</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,40" id="srcline40">40</a></span><span class="line">       <span class="mxinfo" id="T9:U137"><span class="var type1" id="S4T9U209">a</span>=<span class="mxinfo" id="T9:U139"><span class="var type1" id="S13T3U211">X</span>(<span class="mxinfo" id="T6:U141">3</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,41" id="srcline41">41</a></span><span class="line">       <span class="mxinfo" id="T9:U142"><span class="var type1" id="S5T9U215">a_a</span>=<span class="mxinfo" id="T9:U144"><span class="var type1" id="S13T3U217">X</span>(<span class="mxinfo" id="T6:U146">4</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,42" id="srcline42">42</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,43" id="srcline43">43</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,44" id="srcline44">44</a></span><span class="line">       </span></span>
</pre>
